#include "slepbase.h"
#include "epph.h"

#include <assert.h>
#include <math.h>


int nnLeastC(slep_matrix* A, slep_vector* y, double z, OPTS *opts,
			slep_vector* x, slep_vector* funVal){

	slep_vector *ATy, *b1, *Ax, *x1;
	double rsL2, lambda;
	int i;

	assert( y->dim == A->m);
	assert( z > 0.0);

	sll_opts(opts);

	initNormalization(A,opts);

	//assert( !(A->sparsity==SlepDense && opts->nFlag!=0) );

	Ax = slepAllocDenseVector(A->m);
	ATy = slepAllocDenseVector(A->n);
	b1 = slepAllocDenseVector(y->dim);

	NORMALIZEDMTV(A,y,ATy,opts,b1);

	if( hasGot(opts,SlepOPT_rsL2) )
		(rsL2=opts->rsL2), assert( rsL2 >= 0.0);
	else
		rsL2 = 0.0;

	if( opts->init ==  2)
		slep_zerov(x);
	else{
		if(hasGot(opts,SlepOPT_x0)){
			assert(opts->x0->dim == A->n);
			slep_dvcopy(opts->x0,x);
		}else
			for(i=0; i<ATy->dim; i++)
				x->data.vctr[i] = fabs(ATy->data.vctr[i]);
	}

	Ax = slepAllocDenseVector(A->m);
	x1 = slepAllocDenseVector(x->dim);

	NORMALIZEDMV(A,x,Ax,opts,x1,i);

	if( opts->init== 0){
		double x_norm, x_2norm;
		x_norm = slep_dasum(x);
		x_2norm = slep_ddot(x,x);
		if( x_norm >= 1e-6){
			;
		}
	}

	/* The main program */
	/* The Armijo Goldstein line search scheme */

	if( opts->lFlag == 0){
		int bFlag, iterStep;
		double gamma, l_sum, r_sum;
		slep_vector *xp, *xxp, *Axp, *s, *As, *v, *ATAs, *g, *Av, *Axy;
		double t, tp, alpha;
		double lambda0;
		int zf_step;
		slep_vector *xx, *vv;

		bFlag=0;
		gamma = 1.0 + rsL2;

		xp = slepCopyToNewVector(x);
		Axp = slepCopyToNewVector(Ax);

		s = slepAllocDenseVector(x->dim);
		v = slepAllocDenseVector(x->dim);
		xx = slepAllocDenseVector(x->dim);
		vv = slepAllocDenseVector(x->dim);
		g = slepAllocDenseVector(x->dim);
		ATAs = slepAllocDenseVector(x->dim);
		As = slepAllocDenseVector(A->m);
		Av = slepAllocDenseVector(A->m);
		Axy = slepAllocDenseVector(A->m);

		xxp = slepAllocDenseVector(x->dim);
		slep_zerov(xxp);

		tp=0.;
		t=1.;

		lambda0 = 0.0;

		for( iterStep=0; iterStep < opts->maxIter; iterStep++){

			alpha = (tp-1.0)/t;

			slep_dvcopy(x,s);
			slep_daxpy(alpha,xxp,s);

			slep_dvcopy(Ax,As);
			slep_daxpby(-alpha, Axp, 1.0+alpha, As);

			NORMALIZEDMTV(A,As, ATAs, opts, b1);

			slep_dvcopy(ATAs,g);
			slep_daxpy(-1.0,ATy,g);
			slep_daxpy(rsL2,s,g);

			slep_dvcopy(x,xp);
			slep_dvcopy(Ax,Axp);

			for(;;){
				slep_dvcopy(s,v);
				slep_daxpy(-1.0/gamma,g,v);

				for(i=0; i < v->dim; i++)
					vv->data.vctr[i] = (v->data.vctr[i]>=0? v->data.vctr[i] : 0.0 );

				eplb(xx->data.vctr, &lambda, &zf_step, vv->data.vctr, vv->dim, z, lambda0);
				lambda0 = lambda;

				for(i=0; i < x->dim; i++)
					x->data.vctr[i] = (v->data.vctr[i]>=0? xx->data.vctr[i] : 0.0 );


				slep_dvcopy(x,v);
				slep_daxpy(-1.0,s,v);

				NORMALIZEDMV(A,x,Ax,opts,x1,i);

				slep_dvcopy(Ax,Av);
				slep_daxpy(-1.0,As,Av);

				r_sum = slep_ddot(v,v);
				l_sum = slep_ddot(Av,Av);

				if(r_sum <= 1e-20){
					bFlag=1;
					break;
				}

				if( l_sum <= r_sum * (gamma - rsL2))
					break;
				else
					gamma = max( 2.0*gamma, rsL2 + l_sum / r_sum );
			}

			tp = t;
			t = (1.0+sqrt(4.0*t*t + 1.0))/2.0;

			slep_dvcopy(x,xxp);
			slep_daxpy(-1.0,xp,xxp);

			slep_dvcopy(Ax,Axy);
			slep_daxpy(-1.0,y,Axy);

			funVal->data.vctr[iterStep] = 0.5*(slep_ddot(Axy,Axy)+ rsL2 * slep_ddot(x,x) );

			if(bFlag)
				break;

			if( opts->tFlag == 0){
				if( iterStep >= 1){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=1){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dnrm2(xxp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dnrm2(xxp) <= (opts->tol) * max(slep_dnrm2(xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeVector(xp);
		slepFreeVector(xxp);
		slepFreeVector(Ax);
		slepFreeVector(Axp);
		slepFreeVector(s);
		slepFreeVector(As);
		slepFreeVector(v);
		slepFreeVector(ATAs);
		slepFreeVector(g);
		slepFreeVector(Axy);
		slepFreeVector(xx);
		slepFreeVector(vv);
		funVal->dim = iterStep;
	}

	slepFreeVector(ATy);
	slepFreeVector(b1);
	slepFreeVector(Ax);
	slepFreeVector(x1);
	return 0;
}